#ifndef ROBOTICARMCTRL_H
#define ROBOTICARMCTRL_H

#include "CEvtVar.h"
#include "PINGGER.h"
#include "elite_robot_sdk.h"

#define ARM_JBI_NOT_DONE                        (0)     //JBI未工作完成
#define ARM_JBI_DONE                            (1)     //JBI工作完成
#define ARM_JBI_NONE                            (255)   //JBI检测值未知


#define ARM_JBI_SIDE_LEFT                       (1)     //JBI左侧
#define ARM_JBI_SIDE_RIGHT                      (2)     //JBI右侧

#define ARM_JBI_CMD_IADDR                       (1)      //JBI命令寄存器地址
#define ARM_JBI_RET_IADDR                       (2)      //JBI返回值寄存器地址
#define ARM_JBI_PARAM1_IADDR                    (3)      //JBI参数1寄存器地址

#define ARM_JBI_X_OFFSET_DADDR                  (21)     //JBI参数:机械臂X轴偏移寄存器地址
#define ARM_JBI_Y_OFFSET_DADDR                  (22)     //JBI参数:机械臂Y轴偏移寄存器地址
#define ARM_JBI_Z_OFFSET_DADDR                  (23)     //JBI参数:机械臂Y轴偏移寄存器地址

#define ARM_JBI_CMD_MOVE_OUT_BOX_TIME            (120*1000)
#define ARM_JBI_CMD_MOVE_BACK_BOX_TIME           (120*1000)
#define ARM_JBI_CMD_CHARGE_TIME                  (120*1000)
#define ARM_JBI_CMD_DRAW_TIME                    (120*1000)
#define ARM_JBI_CMD_SHOT_TIME                    (60*1000)
#define ARM_JBI_CMD_MOVE_OFFSET_TIME             (120*1000)

enum ARM_JBI_CMD
{
    ARM_JBI_CMD_MOVE_OUT_BOX=1,                           //设置机械臂从框内移出
    ARM_JBI_CMD_MOVE_BACK_BOX=2,                          //设置机械臂收回框内
    ARM_JBI_CMD_CHARGE=3,                                 //设置机械臂插枪
    ARM_JBI_CMD_DRAW=4,                                   //设置机械臂拔枪
    ARM_JBI_CMD_SHOT=5,                                   //设置机械臂拍照位姿
    ARM_JBI_CMD_MOVE_OFFSET=6,                            //设置机械臂（x,y,z）偏置移动
    ARM_JBI_CMD_RESTART_LUA=7,                            //重启Lua脚本
    ARM_JBI_CMD_FIXED_POINT_CHARGE=8,                     //插枪跑固定点插枪
    ARM_JBI_CMD_FIXED_POINT_DRAW=9,                       //插枪跑固定点拔枪
    ARM_JBI_CMD_MOVE_CHARGE_CENTER=10,                    //移动充电口到视觉点中心
    ARM_JBI_CMD_MOVE_FAILED_BACK_BEFORE=11,               //设置失败时，置机械臂收回框内前的姿态
};

class roboticArmCtrl
{
public:
    roboticArmCtrl();
    ~roboticArmCtrl();

    bool connectArm();
    bool is_connected();

    int set_servo_on(bool& reslut);                                             //设置伺服使能
    int set_servo_off(bool& reslut);                                            //设置伺服下使能

    int stop(bool& optRet);                                                     //停止机器人运行
    int setSpeed(double speed_value, bool& reslut);                             //设置机械臂速度
    int runJbi(std::string filename, bool& reslut);                             //运行jbi文件
    int setCollisionEnable(ELITE_SDK_SWTICH col_switch, bool& reslut);          //关闭碰撞使能

    int jbi_arm_move_out_box(bool& reslut, int timeoutMS);                      //设置机械臂从框内移出
    int jbi_arm_move_back_box(bool& reslut,int timeoutMS);                      //设置机械臂收回框内
    int jbi_arm_charge(bool& reslut,int side, int timeoutMS);                   //设置机械臂插枪
    int jbi_arm_draw(bool& reslut,int side, int timeoutMS);                     //设置机械臂拔枪
    int jbi_arm_shot(bool& reslut,int side, int timeoutMS);                     //设置机械臂拍照
    int jbi_arm_move_offset(bool& reslut,\
                            double offsetX,double offsetY,double offsetZ, int timeoutMS);//设置机械臂（x,y,z）偏置移动
    int jbi_arm_restart_lua(bool& reslut,int timeoutMS);                        //设置机械臂重启Lua脚本
    int jbi_arm_fixed_point_charge(bool& reslut,int timeoutMS);                 //插枪跑固定点插枪
    int jbi_arm_fixed_point_draw(bool& reslut,int timeoutMS);                   //插枪跑固定点拔枪
    int jbi_arm_move_charge_center(bool& reslut,int timeoutMS);                 //移动充电口到视觉点中心
    int jbi_arm_move_failed_back_before(bool& reslut,int timeoutMS);            //设置失败时，置机械臂收回框内前的姿态


    PINGGER pingger;

    int robot_speed;                                                            //机器人运行速度
private:
    char robotIP[16];
    int  robotPort = {8055};

    bool mInited =false;
    void print_base_info();
    void loadINI();
    EliteRobotSdk elite_robot;

    int jbi_arm_send_cmd_get_reslut(bool& reslut,int cmd, int timeoutMS);

};

#endif // ROBOTICARMCTRL_H
